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ABST RA CT. An efficient Lagrangian formulation of manipulator dynamics has been developed. The 
efficiency derives from recurrence relations for the velocities, accelerations, and generalized forces. The num¬ 
ber of additions and multiplications varies linearly with the number of joints, as opposed to past Lagrangian 
dynamics formulations with an n 4 dependence. With this formulation it should be possible in principle to 
compute the Lagrangian dynamics in real time. The computational complexities of tlvis and other dynamics 
formulations including recent Newton-Euler formulations and tabular formulations are compared. It is con¬ 
cluded that recursive formulations based either on the Lagrangian or Newton-Euler dynamics offer the best 
method of dynamics calculation. 
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Introduction 


The inverse problem for manipulator dynamics, namely the problem of computing die joint torques 
required to produce given joint positions, velocities, and accelerations, has until recently been a computational 
bottleneck in the control of manipulators. Past formulations of manipulator dynamics have been based on a 
relatively inefficient derivation from die Lagrange equations [1,2], The solutions to these equations required on 
the order of seconds on a computer for each trajectory point, and it was perceived diat therefore torques for 
trajectories could not be computed in real time. 

Two different types of schemes were proposed to render the Lagrangian dynamics computationally 
feasible: (1) simplifying the dynamics by ignoring some terms and correcting errors with feedback, and (2) 
tcssellating the state space along various dimensions and tabulating part of the solution. The most common 
method of simplifying the dynamics has been to ignore the Coriolis and centrifugal forces, which are by far the 
greatest computational burden in the dynamics calculation [3,4]. However, ignoring Coriolis and centrifugal 
forces works well only at slower speeds of movement; at fast speeds of movement die Coriolis and centrifugal 
forces are a major component of the dynamics [5]. The errors in the computed torque resulting from ignoring 
Coriolis and centrifugal forces cannot be corrected with feedback because of excessive requirements on the 
corrective torques [6]. 

Tabular solutions to the dynamics seek to achieve economies in computation time at the expense of large 
memories. Raibert [7] has discussed diis space-time tradeoff and has examined the different dimensions along 
which the tabularization can be set up. Albus [8,9] represents die space extreme by including in his table all 
dimensions ( q, q, q), representing n dimensional position, velocity, and acceleration vectors. Since die general¬ 
ized forces have die form Fi = f{q, q, q), these forces arc obtained at no computational cost by indexing the 
table with die desired trajectory values (q, q, q). Raibert [10] eliminated the acceleration dimension from die 
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table by expressing the dynamics in the form F t — J{q)q -f K(q, q) and tabulating the position and velocity 
dependent terms J(q) and K(q,q). Because of the large memory costs involved in the above two methods, 
Horn and Raibert [6] proposed a configuration space control that tabulates only the position dependent terms. 
Their formulation will be presented later in conjunction with a discussion of its computational complexity. 
The difficulties with tabular methods include (1) enormous table sizes necessitated by creating a fine enough 
tessellation along the various dimensions, (2) filling the entries in the table, (3) interpolating between stored 
values, and (4) the tables become useless if the load changes suddenly such as when an object is picked up. 


This paper presents a reformulation of die Lagrangian dynamics for a manipulator that greatly reduces 


the computational burden and that should allow real time computation of the inverse dynamics. Specifically, 
there are three parts to the reformulation: 

1. backward recursion of the velocities and accelerations working from the base of the manipu¬ 
lator to the end link, 

2. forward recursion of the generalized forces working from die end link to die base of the 
manipulator, and 

3. use of 3x3 rotation matrices instead of 4x4 rotation-translation matrices and homogeneous 
coordinates. 


The end result of applying these methods is diat the number of additions and multiplications varies linearly 
with die number of joints. This reformulation brings the Lagrangian dynamics into line with recently 
developed recursive Newton-Ruler dynamics [5,11,12], whose computational complexity varies linearly with 
the number of joints and whose formulation contains parts 1 and 2 above. Part 3 was delineated in [12] for 
the Newton-Ruler mcdiod, and a similar preference for 3x3 versus 4x4 matrices was expressed for reasons of 
computational efficiency. 


The Uicker/Kahn Lagrangian Formulation 


The standard formulation for manipulator dynamics is derived from Uickcr [1], who used 4x4 matrices to 
set up the Lagrangian based dynamics for a somewhat more general linkage problem. This formulation was 
particularized to open loop kinematic chains by Kahn [2], Borrowing Kahn’s notation (Figure 1), the links of 
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li^ure 1 . Standard coordinate axes definitions for connected links and relationships between 
neighboring coordinate axes. 


a manipulator arc numbered consecutively from 1 to n starting from the base to the tip. By convention the 
reference frame is numbered link 0. The joints are the points of articulation between links, and are numbered 

so that joint i connects links i — 1 and i. An orthogonal coordinate system is fixed in each link as follows: 
z i is directed along the axis of joint i + 1, 

Xi lies along the common normal from x to z iy and 
Vi completes the right handed coordinate. 


I he ielative position of two adjacent links is completely described by the following four parameters: 

a i is the distance between die origins of coordinate systems i — 1 and i measured along x iy 


H is die distance between £ 2 _i and x t measured along z, 


i —1 


a i is the angle between tlic ^—i and zi axes measured in a righthand sense about Xi , and 
&i is die angle between tlie and the x t axes measured in the righthand sense about^.- 


*—i 


If the joint is rotational die joint variable will be if translational the joint variable will be s>. The symbol 
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<7 i will designate the variable for joint i whether it is s ; or 0 t . The vector Q = (qi,q 2 ,..., q n ) represents the 
generalized coordinates of the manipulator and completely specifies the position. Multiple rotational degrees 
of freedom can be expressed by joints with zero mass and zero length links. 

Let l vj = (1 x y z) r be a vector from coordinate system i to a point fixed in link j expressed in link i 
coordinates. Then adjacent coordinate systems are related by die relation: 


i —1 


vj — Ai ’ vj 


where Ai is the 4x4 transformation matrix between coordinate systems i — 1 and t: 


1 

0 

0 

0 

a, cos Oi 

cos 0, 

sin#* cos a* 

sin Oi sin a* 

cii sin Oi 

sin Oi 

cos#* cos a* 

— cos #* sin a* 

Si 

0 

sin a* 

cos a* 


We define A 0 = I, the identity matrix. Any two coordinate systems i and j can be related by cascading the 
transformations: 


l v k = % 4 fc (3) 

where l Wj = Ai+iAi+ 2 - ■ ■Aj for i < j. We define ] Wj = I. When referring to the base coordinate system 
we omit the 0 superscript, so that v k — °v k and W 3 = °Wj. 

Using tliis notation, the generalized forces F t for an n-link manipulator have been derived from the 
Lagrange equations [1,2]: 


" JL, / {dW dWT\\ 4 J 


(dWj d 2 wj\ 

/r ( ~dqi J] dq k d qi ) m 


T dW, . 

m J 9 Jr J ( 4 ) 
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where 

Jj is die inertia tensor with respect to the proximal joint of link j expressed in j's body coor¬ 
dinates, 

m j is the mass of link j , 

9 is the gravity vector, and 

■Vj is the coordinate of the center of mass of link j expressed in link j's body coordinates. 
Evaluation of (4) has been considered too slow for real time computation. As an estimate of the complexity 
of computing the dynamics in this formulation, the total number of additions and multiplications has been 
calculated. In arriving at a number, die operations in (4) were carried out more or less as set forth, because any 
efficiencies in calculating these terms should be explicit in the formulation rather than hidden in a program. A 
few reasonable economies nevertheless were practiced. Simple decompositions saved some recomputation. 


dWi 

<%• 

oHVj 

dqkdqi 


k Wj 

Aq k 

k<j 

= W k - i^ fc % 

oq k dqi 

k<l<j 

=nt_A 2 t “Wj 
d % 

k — l 



The terms k Wj were computed first and stored. Then the dWj/dq k and d 2 Wj/dq k dqi terms were computed and 

their results also stored. The cost of computing the terms in the matrices Aj,dAj/dqj, and d^A j/dq] was not 

•/ 

included, since drey depend on the particular manipulator configuration and because they contribute only a 
small linear cost to die computational complexity. Finally, no allowance has been made for any sparseness of 


die matrices. 


Table I shows that the Uickcr/Kahn formulation gives an n A dependence on the number of multiplica¬ 
tions and on the number of additions. Table II shows that for n — 6 dicse numbers arc 66,271 and 51,548 
respectively. These large numbers of operations are too time consuming to compute in real time. Lull et al. [5] 
have estimated that to compute die torques for one nominal point in the trajectory requires 7.9 seconds on a 
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Table I. Comparison of Dynamics Formulations 

Method 

Multiplications 

Additions 

Uicker/Kahn 

32 jn 1 -f- 86 5/12n 3 -f-171 \n 2 

+53 in — 128 

25+ + 66 |+ + 129 2 + 

+42 in —96 

Waters 

106.1++ 620 ^-512 

82+ + 514n — 384 

Hollcrbach(4x4) 

830n — 592 

675n- 464 

Hollcrbach(3x3) 

412n — 277 

320n 201 

Newton-Ruler 

150n — 48 

13 In — 48 

Horn, Raibert 

2 n 3 -f~ n 2 

-f- n 2 -|- 2n 


Fable II. Comparison for n — 6 

Method 

Multiplications 

Additions 

Uicker/Kahn 

66,271 

51,548 

Waters 

7,051 

5,652 

Hollcrbach(4x4) 

4,388 

3,586 

Uollerbach(3x3) 

2,195 

1,719 

Newton-Euler 

852 

738 

Horn, Raibert 

468 

264 


PDP11/45. 


Lagrangian Dynamics with Backward Recursion 

Waters [13] first noticed that the generalized forces (4) could be expressed in a form that allowed one 
to take advantage of several backward recurrence relations (backward in the sense of die link numbering direc¬ 
tion). We present below a simpler formulation and derivation of these recurrence relations. In reexamining the 
derivation of the generalized forces it becomes apparent that the generalized forces (4) can be expressed in a 
more compact form: 



• « 

where Wj could be expanded to yield the tisual form for the components of the reaction and die Coriolis and 
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centrifugal forces: 


Wj 


> —- 
dqk 


'ik I 


“ ~ <*&<** 


(7) 


It is however best to leave this term uncxpandcd. With tliis more compact formulation the specific recurrence 
relations for the velocities Wj and accelerations W 3 arc easily derived by straightforward differentiation: 


Wj = Wj-iA,- 

Wj — Wj-\A } + Wj—nAj 
. dAj 

-Wj^Aj + Wj^j 


Wt 


dA 


d f dA, 


W 3 ^A 3 + 4 - Wj-^j + ^ + Wj 


dt \ dq. 


l 


dAj 

j •• 



dA< 

^ m ^eT 3 


4, + Wj_ 




dA± 

dq 3 


• * 



( 8 ) 

(9) 



With this formulation tine number of additions and multiplications is reduced to an n 2 dependence 
(Tables I and II). For n — 6 there arc 7051 multiplications and 5652 additions. The reduction from an 
7i 4 to an n 2 dependence comes about primarily from a more efficient Coriolis and centrifugal force computa¬ 
tion; that is to say, this formulation requires only calculation of d 2 W i /dq 2 instead of all of the matrices 
d 2 W 3 /dq k dq. 


Forward Recursive Lagrangian Dynamics 

An additional level of forward recursion can be placed on the generalized force formulation (6), leading 
to further efficiency of the Lagrangian dynamics. We note that SW 3 /dqi == dW,/dq, { W 3 . The generalized 
force equation (6) can then be written: 
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n r 


Fi 


E 


■ 1 


SW • •• T 

t r [ 1 <W JW 

dq t 




T m i 


<3q, 


Wjirj 



This reformulation lends itself to forward recursion as follows: 



n 


T 


Di = 2 'V^VV. 

j=i 


‘Wjtf + £ A i+I l |1 fT J ,/ J V^. 

j==2-fl 


n 


r 


r 


JiWi /Ij-j-i-Dj-j-i 




n 

mj 'Wjhj 

j—i 



= Vj + /i i+1 cj +1 



Substituting into (11), the generalized forces become simply: 



This recursive formulation is computed similarly to recursive formulations of the Ncwton-Fulcr dynamics 

•• T 

[5,11,12]. First ail the W i terms are computed staring from i = 1 to i — n using the recurrence relations (6). 
r fhcn the Di and c, terms arc computed in the other direction, from i — n to i = 1. With this formulation 
there are now 830n — 592 multiplications and 675n — 464 additions; for n = 6 there arc 4388 multiplica- 
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tions and 3586 additions. This linear computation cost has essentially been obtained for free, eliminating die 
n 2 cost in Waters’ formulation without significantly changing die coefficients of the other terms. 


Recursive Lagrangian Dynamics with 3x3 Matrices 


Since the previous formulation reduced the computational cost to an n dependence, all that can be done 


is to reduce the size of the coefficients. The greatest reduction can be obtained through a reformulation of the 


Lagrangian dynamics in 


terms of 3x3 matrices rather than 4x4 rotation-translation matrices. 


Although more 


convenient in setting up the dynamics, 4x4 matrices arc inefficient because of some sparseness and because 


of the combination of translation with rotation (sec c.g. [12]). While it is possible to have special purpose 


multiplication routines which take into account the 


zeros and ones of the last row' of the 4x4 transformation 


matrices, it has consistently been the position of this paper that economies in computation should be explicit 


in the formulation rather than implicit in the programming. 
27 multiplications while 4x4 matrix multiplications require 64, 
coefficients of the computational cost terms. 


Because 3x3 matrix multiplications require 
we get a greater than 50% reduction in die 


We suppose that A, now represents a 3x3 rotation matrix relating the orientations of coordinate system 


j — 1 and j. That is to say, if j v is a vector expressed in terms of die orientation of coordinate system j 
axes,then 3 ~ l v = Aj 3 v. When a vector is presented without a left superscript, it is referred to die base 
coordinate orientation (v — °u). In the subsequent development, capital letters represent 3x3 matrices, lower 
case letters represent 3x1 vectors. J W k is defined as before, save diat it is now the composition of 3x3 rotation 


matrices. Define the following vectors (sec Figure 2): 

Pi is a vector from the base coordinate origin to the joint i coordinate origin, 

p* is a vector from coordinate origin i — 1 to coordinate origin i, 

r i is a vector from the base coordinate origin to die link i center of mass, 

r* is a vector from coordinate origin i to the link i center of mass, and 
% 
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link i 

center of mass 


link i-1 
origin 


link i 
origin 


P i-1 


base 

origin 


Figure 2. Vector definitions between the base origin and the link origins and center of masses. 


The generalized forces for this system are derived in Appendix A: 


J = i 


dp ,.. 




v-r , dWj . T dW; .. r 

Wj -|- --i ^ + ^j jWj 


T dWj . 
m * ^ 


(15) 


* • 

The Wj term has the same recursive expression as in equation (10), though presently referring to a 3x3 rotation 
matrix. Thcp term has the following recurrence relations: 


Pj = Pj_i — iy, V. 


(16) 


Lastly, in Appendix B the forward recursive relation for D, is derived. The recurrence relation for c,- is the 
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same as (13). 



(17) 


where 



—+ l Pi-\-\ ^i-\- 1 + ln iPi 4 - JiW i 


«.■ =«i+i + rriipj -f l n[ W • 


r, 


7 ' 



This formulation decreases the coefficients of the complexity polynomial of the recursive Lagrangian formula¬ 
tion with 4x4 matrices by better than 50% (Tables I and II). For n = 6 there are now 2195 multiplications and 
1719 additions. 


Recursive Newton-Killer Dynamics 

The Lagrange equations represent only one of several possible approaches towards deriving the dynamic 
equations for open loop kinematic chains. Other approaches include Newton-Euler formulations [5,11,12,14,- 
15,16,17,18], a Lagrangian formulation of D’Alembert’s Principle [19,20], and virtual work formulations [21]. 
Various dynamics formulations have been contrasted and compared in [22]. 

Recently a number of papers have appeared on an efficient recursive Newton-Euler formulation of 
manipulator dynamics [5,11,12]. Hooker and Margulics [14] and Hooker [15] presented an early application of 
Newton-Euler equations to open-loop kinematic chains such as satellites. Stepanenko and Vukobratovic [16] 
developed a recursive Newton-Euler formulation in the context of human limb dynamics. This formulation 
was revised and elaborated in subsequent papers [17,18]. 

We present a revision of the Newton-Euler equations as presented in [5]. The backward recursion 
propagates angular velocities, angular accelerations, linear accelerations, total link forces, and total link 
torques from the base to the end link. For a rotational joint, the recursive equations are: 
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4 “ iji 

U)i —Wi— i 4" Zi—lQi 4~ w i— 1 X Zi—\qi 

Pi —Wi X P t + LOj X (OJ, X P ( ) -f Pj_| 

Ti =W; X (tOi X r ,) "1“ 4 X fj 4“ Pi 

F { —m-ri 

Ni —Iitili 4 - Wi X (hiO,) 

where previously undefined terms are: 

is the angular velocity of link i, 
u)i is the angular acceleration of link i, 

Fi is the total external force on link i, 

Ni is the total external torque on link i, and 
h is the inertia tensor of link i about its center of mass. 

The forward recursion propagates the forces and moments exerted on link i by link i — 1 from the end link of 
the manipulator to the base. 


Si ~F l + f i+l 

Hi 1 4- Ni 4- (Pi + r { ) X Fi 4- P* X /i+i 

f =Zi~_ i • m 


where 


fi is the force exerted on link i by link i — 1, 
n i is the moment exerted on link i by link i — 1, and 
T * is the input torque at joint i. 


In this fot initiation the implicit reference coordinate frame is the base coordinates. In the next section a more 


efficient reference frame is examined. 
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Recursive Newton-Euler Dynamics Referred to Link Coordinates 


Orin ct al. [12] initially proposed that the forces and moments in the Newton-Euler formulation of [16] 
be referred to the link’s internal coordinate system. Armstrong [11] and Luh et al. [5] extended this idea by 
calculating the angular and linear velocities and accelerations in link coordinates as well. The end result of 
lcfcicncing both the dynamics and kinematics to the link cordinatcs is to obviate a great deal of coordinate 
transformation and to allow the inertia tensor to be fixed in each link coordinate system. 

Although Aimstiong s and Luh s formulations are equivalent, they addressed complementary problems. 
Armstrong sought to integrate the dynamic equations to find the accelerations, given the applied torques. 
Luh solved the inverse problem, namely to find the appropriate joint torques given desired joint positions, 
velocities, and accelerations. Another difference is that Armstrong’s dynamics arc referred to coordinate sys¬ 
tems located at the joints, while Lull’s dynamics are referred to coordinate systems located at the link centers 
of mass. 

Using Lull’s formulation once again, and rather than rewriting all the Newton-Euler equations showing 
how they arc referred to internal link coordinates, we present 3 examples of this reformulation. For a more 
complete presentation, the reader is referred to [5]. 


u i —0 l Wi —i -j- 1 l Zi^iqi) 


i —I 


'Ni = l I t l dH + *Wi X (*IM) 
i fi= i F i + A i+l ( i + l f i _ hl ) 


where A] = (A,) and 7 v denotes a vector v represented in the orientation of coordinate system j 
mcasuicd from the base oiigin. From lablcs 1 and II it is seen that the recursive Newton-Euler formulation 
gives a 60% reduction in additions and multiplications over the recursive Lagrangian formulation. For n == 6 
there arc only 852 multiplications and 738 additions. 
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The Configuration Space Method 

The Configuration Space Method [6] is derived from the following reformulation of the Lagrangian 


dynamics: 


n n 


F i = g&) + E J >M<h + E E c ’.mAk 

i=i i=i fc=t 


I he gravity compensation G,, inertial terms J, r , and Coriolis coefficients are tabulated in this 
formulation. The number of additions and multiplications using the Configuration Space Method are 
presented in Tables I and IT. Although there is still an n 3 dependence for the additions and multiplications, 
the much smaller coefficients on the polynomial terms represent a greatly reduced computational cost. For 
n = 6 there arc just 468 multiplications and 264 additions. An additional saving not indicated in these figures 
is that it is unnecessary to compute the sines and cosines of angles. However, for n > 9 the Configuration 
Space Method becomes less efficient than the Newton-Eulcr formulation. 


Conclusions 


The problem of computing manipulator dynamics in real time appears to have been solved. With both 
the recursive Lagrangian formulation and the recursive Newtonian formulation of tire dynamics, the number 
of additions and multiplications varies linearly with the number of joints. For 6 joints the number of additions 
and multiplications should be computable in real time with even modest computers such as PDPLl’s. The 
impetus for applying approximations or tabular techniques to solve tire dynamics appears to have been lost. 
With respect to the Configuration Space Method in particular, which for n < 9 joints is the most efficient 
formulation, problems incumbent with the use of tables such as large memories, configuration sensitivity, 
interpolation, and filling the tables arc obviated. 

It appears unlikely that any but very minor improvements in efficiency can be made to the present recur¬ 
sive formulations. The recursive Newton-Euler formulation is more efficient than the recursive Lagrangian 
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formulation presented here. One reason for presenting this recursive Lagrangian formulation is to demonstrate 
that the Lagrangian formulation is not necessarily so computationally intensive as to preclude real time com¬ 
putation, as had been the assumption for the past 10 years [2,3,4,6], and to demonstrate that the Lagrangian 
formulation can be made roughly as efficient as the Ncwton-Eulcr formulation [5], For some applications 
involving the use of homogenous coordinates and 4x4 transformation matrices, e.g. [23], the recursive 
Lagrangian formulation may be the most convenient efficient dynamics formulation. 
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Appendix A 

In this appendix the kinetic energy for a link is derived, and the Lagrange equations are solved. Let hi be 
a vector from the base origin to a point fixed in link i. h* a vector from coordinate system i to the same point. 
Then 


hi = Pi + VL; % 
hi =Pi + Wi % 


(Al) 

(A 2) 


The kinetic energy for a particle on link i is given by: 


k; 


~tr(hihj^dm 

\tr(pipj -f 2 Wi %pj + Wi % % T W r [)dm 


Integrating over all particles in link i, the total kinetic energy Ki of link i is given by: 


(A3) 


Ki - l 2 2 tr(m lPl pJ + 2VT i i n t pJ’ + W^Wj) 


(A 4) 


where m.; is tire mass of link i, l rii — / ’h*drn and l ni/m t is the vector to the center of mass, and J, 


T 


f l h\ l h* dm is the inertia tensor with respect to the coordinate system i. The total kinetic energy for all the 
links is given by: 


n 




1 — 

2 E tr ( m jPjP}' + 2 Wj j njpJ + WjJjWj) 
i=l 


(A 5) 
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Appendix A 


The potential energy derives from gravity and is equal to the sum of the work required to displace the mass 
center of each link from the horizontal reference plane: 


P = P-J2 m >9 T ^ Vj 

j~ 1 




where g = {g x g y g z ) is the acceleration due to gravity and P is a constant which depends on the particular 
reference plane chosen. The Lagrangian L is defined as the difference between the kinetic and potential 
energies: L = K — P. The Lagrange equations are: 


dL 


1 f • • . f Tb 


(A7) 


Since the potential energy is position dependent only. 


6K_ dP 

dqi + <% 


(48) 


The terms are calculated below. 


3 = 1 


E tr \ m ^pJ + m J 


mV, . 




• T i 3 i -*T 


+ + 


dp] 


dt\ dqi Y 3 3 


(49) 


dK 

dqi 




(410) 


m 


qp 

dqi 




T dWj . 

_ i 3. 


J=i 


(411) 
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Appendix A 


It can be shown that 


<% = <% 

% 


dWj _d\Vj 
<% 

<9% J 

Substituting into the Lagrangian, and noting that tr[AB r ) = tr(BA T ), 





(A 12) 


n 




= t L 


<9pi. lT , <% j n T^y T _|_ uT , 


f n m i—Pi + 


% 


<9% 


<9W,. rp aw j .. T 

] njp] + —/.W 


<9<7 j 




3 


T dWj . 

m /J -JZr Jr 3 


dqi 


C-413) 



n 


Appendix B 


In this appendix the forward recursive relations are developed. For i < j, 


Substituting into (15) and factoring. 


(Bl) 


tr {^7 + Wn^w] + 

J = l 


dWi 




3~t 


(B2) 


The terms inside the summations may be computed by recurrence relations. 


JlU/pfpJ + + tWjinjp J + ‘WjJjW*} 

j=i ' 

{( A i+1 i+l PJ + Wi)("VpJ + j nJW^ + 4 +1 ’+' + 'ntf + J t W\ 


A’+iA+i 4- J Pi-H e »-hi ln ilpJ + Ji^i 


m 


where 


J2 ( m #J + S T ^i) 


(B4) 


■e i+ i + tthpI + * n F W 
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Appendix B 


The recurrence relation c t for the gravity term is the same as equation (13), except for the dimensionality 
difference. Substituting into (B2), the generalized force is: 








